package clubhouse.motioncommand;

import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.lang.Math;
import java.util.HashSet;
import java.util.List;
import java.util.ListIterator;
import java.util.Vector;

import clubhouse.base.Base;
import clubhouse.base.Landmarks;
import clubhouse.control.DataChecker;

import clubhouse.voronoi.Pnt;
import clubhouse.voronoi.Triangle;
import clubhouse.voronoi.Triangulation;

public class DecisionMaker {
	private static final double robotClosestPtThreshold = 0.2;
	private static final double verticalThreshold = 2.3;

	public DecisionMaker(int scenario){
		if ((scenario == 2)||(scenario == 3)){
	        initialTriangle = new Triangle(
	                new Pnt(-initialSize, -initialSize),
	                new Pnt( initialSize, -initialSize),
	                new Pnt(           0,  initialSize));
	        this.dt = new Triangulation(initialTriangle);
		}
	}
	
	private double euclideanDist(double x1, double y1, double x2, double y2){
		return(Math.sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)));
	}
	
	/*
	 * 
	 * 
	 * Scenario 1
	 * 
	 * 
	 * 
	 * 
	 */


	public Integer getNextIndex(double x_robot, double y_robot, double[] previous_dest,Integer current_index){
		double distance = 0;
		if (Base.DEBUG) System.out.println("(getNextIndex) x_robot="+x_robot+", y_robot="+y_robot+", previous_dest[1]="+previous_dest[1]+", previous_dest[2]="+previous_dest[2]);
		distance = this.euclideanDist(x_robot, y_robot, previous_dest[1], previous_dest[2]);
		if (Base.DEBUG) System.out.println("(getNextIndex) distance robot/previous dest = "+distance);
		if ((distance <= 0.5) && (distance >= -0.5)){
			if (current_index == 4){
				current_index = 1;
			} else {
				current_index = current_index + 1;
			}
		} // else keep the same destination

		return(current_index);
	}
	
	/*
	 * 
	 * 
	 * Scenario 2
	 * 
	 * 
	 * 
	 * 
	 */
	//Scenario 2
    private Triangulation dt;                   // Delaunay triangulation
    private Triangle initialTriangle;           // Initial triangle
	public Vector<Point2D[]> polygons = null;
    private static int initialSize = 1000;     // Size of initial triangle
    
	 private Point2D project(Point2D point, Line2D line){
		 Point2D p0 = line.getP1();
		 Point2D p1 = line.getP2();
		 if (point.equals(p0) || point.equals(p1)) return point;
		 double euclDistance = point.distance(p0);	//distance between robot & one bound of the line
		 double d = Math.sqrt(euclDistance*euclDistance-line.ptLineDist(point)*line.ptLineDist(point));	//distance between the projected point & the same bound
		 double xInt,yInt;
		 
		 xInt = p0.getX() + d * Math.cos(Math.atan((p1.getY() - p0.getY())/(p1.getX() - p0.getX())));
		 yInt = p0.getY() + d * Math.sin(Math.atan((p1.getY() - p0.getY())/(p1.getX() - p0.getX())));
		 if (Base.DEBUG) System.out.println("(project) (xInt,yInt) = ("+xInt+","+yInt+")");
		 Point2D coord = new Point2D.Double(xInt,yInt);
		 return coord;

	 }
	 
	private boolean isInside(Vector<Line2D> vector, Line2D line){
		boolean result = false;
		for (int i=0;i<vector.size();i++){
			Line2D vectorLine = vector.get(i);
			result = ((vectorLine.getX1()==line.getX1())&&(vectorLine.getY1()==line.getY1())&&(vectorLine.getX2()==line.getX2())&&(vectorLine.getY2()==line.getY2()));
			if (result) break;
		}
		return result;
	}
	
	private Line2D createSortedLine(Point2D point1,Point2D point2){
		Line2D result=null;
		if (point1.getX()<=point2.getX()){
			result = new Line2D.Double(point1, point2);
		} else {
			result = new Line2D.Double(point2, point1);
		}
		
		return(result);
	}
	  
	private Line2D getDestLine(Point2D robot, Vector<Point2D[]> polygons) throws Exception {
		Line2D result = null;
		
		int b = 0, i = 0, j;

		if (polygons.size() == 2){		//Only 2 polygons, result = common edge = 2 common vertices
			Point2D[] bounds = new Point2D[2];
			if (Base.DEBUG) System.out.println("(getDestLine) Only 2 polygons");

			while ((b<2) && (i<polygons.get(0).length)){		//abscissa' loop
				for (int k=0;k<polygons.get(1).length;k++){		//ordinates' loop
					if (polygons.get(1)[k].equals(polygons.get(0)[i])){
						bounds[b] = polygons.get(0)[i];
						if (Base.DEBUG) System.out.println("(getDestLine) bounds["+b+"] = "+bounds[b]);
						b++;
						break;
					}
				}
				i++;
			}
			if (b==2){
				result = this.createSortedLine(bounds[0], bounds[1]);
			} else throw new Exception("Cannot find 2 common point");
			
		} else {
			//Get closest point & its 2 lines
			Point2D tempPoint = null, closestPoint=null;
			double tempDist, minimaldist = 10000;
			Vector<Line2D> tempLines = new Vector<Line2D>(5,1);
			
			if (Base.DEBUG) System.out.println("(getDestLine) We have "+polygons.size()+" polygons");
			for (i=0;i<polygons.size();i++){
				for (j=0;j<polygons.get(i).length;j++){
					tempPoint = polygons.get(i)[j];
					tempDist = robot.distance(tempPoint);
					
					if (tempDist < minimaldist){
						minimaldist = tempDist;
						closestPoint = tempPoint;
					}
				}//end vertices' for
			}//end polygons' for

			if (closestPoint == null) throw new Exception("Cannot find closest point");
			if (Base.DEBUG) System.out.println("(getDestLine) Closest point : "+closestPoint);
			
			Line2D o = null;
			for (i=0;i<polygons.size();i++){
				for (j=0;j<polygons.get(i).length;j++){
					if (polygons.get(i)[j].equals(closestPoint)){
						if (j==0){	//Be careful with arrays boundaries
							o = this.createSortedLine(closestPoint, polygons.get(i)[polygons.get(i).length-1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
							o = this.createSortedLine(closestPoint, polygons.get(i)[j+1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
						} else if (j==polygons.get(i).length-1){
							o = this.createSortedLine(closestPoint, polygons.get(i)[j-1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
							o = this.createSortedLine(closestPoint,polygons.get(i)[0]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
						} else {
							o = this.createSortedLine(closestPoint, polygons.get(i)[j-1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
							o = this.createSortedLine(closestPoint, polygons.get(i)[j+1]);
							if (!this.isInside(tempLines,o)) tempLines.add(o);
						}
					}
				}
			}
			if (tempLines.size() == 0) throw new Exception("Cannot lines with the closest point");
			
			if (Base.DEBUG) {
				String str = "(getDestLine) ";
				str = str + "The "+tempLines.size()+" lines are : [";
				for (i=0;i<tempLines.size();i++){
					str = str + "[("+tempLines.get(i).getX1()+";"+tempLines.get(i).getY1()+"),("+tempLines.get(i).getX2()+";"+tempLines.get(i).getY2()+")], ";
				}
				System.out.println(str); 
			}
			
			int bestIndex=-1;	
			
			if ((robot.getX()<closestPoint.getX())&&(robot.distance(closestPoint)>DecisionMaker.robotClosestPtThreshold)){ //robot far from the closest point, go to it first
				if (Base.DEBUG) System.out.println("(getDestLine) Robot is far away from the closest point and on its left");

				for (i=0;i<tempLines.size();i++){		//take the line where the closest point is the right bound
					if (tempLines.get(i).getP2().distance(closestPoint)==0){
						
						if (tempLines.get(i).getX1()<=robot.getX()){
							if (bestIndex==-1) bestIndex = i;
							else {
								if (tempLines.get(i).ptLineDist(robot) < tempLines.get(bestIndex).ptLineDist(robot)) bestIndex = i;
							}
						}
					}
				}
				if (bestIndex == -1 ) throw new Exception("Cannot find a correct line where the closest point is the right bound");
				result = tempLines.get(bestIndex);
								
			} else {								//robot close to the closest point, go to the rightest bound
				double slope = 0;
				if (Base.DEBUG) System.out.println("(getDestLine) Robot is on the right of the closest point or close to it");
				for (i=0;i<tempLines.size();i++){		//take the line where the closest point is the left bound
					slope = (tempLines.get(i).getY2() - tempLines.get(i).getY1())/(tempLines.get(i).getX2()-tempLines.get(i).getX1());
					if (Math.abs(slope) > DecisionMaker.verticalThreshold) continue;		//Line too vertical => drop it
										
					//The leftBound is the closestPoint & the closest line
					if ((tempLines.get(i).getP1().equals(closestPoint))){
						if (bestIndex==-1){
							bestIndex = i;
						} else {
							if (tempLines.get(i).ptLineDist(robot)<tempLines.get(bestIndex).ptLineDist(robot)) bestIndex = i;
							//if ( Math.abs((tempLines.get(i).getY1()-tempLines.get(i).getY2())/deltaX) < Math.abs((tempLines.get(indexGoal).getY1()-tempLines.get(indexGoal).getY2())/(tempLines.get(indexGoal).getX1()-tempLines.get(indexGoal).getX2())) ) indexGoal = i;
						}
						
					}
					if (Base.DEBUG) System.out.println("(getDestLine) x_leftBound = "+tempLines.get(i).getX1()+", x_rightBound= "+tempLines.get(i).getX2()+", bestIndex="+bestIndex+", i="+i+", dist="+tempLines.get(i).ptLineDist(robot));
				}
				if (Base.DEBUG) System.out.println("(getDestLine) final bestIndex = "+bestIndex);
				if (bestIndex == -1 )  throw new Exception("Cannot a correct line where the closest point is the left bound");
				result = tempLines.get(bestIndex);
			}
		}
		if (Base.DEBUG) System.out.println("(getDestLine) The chosen line is : [("+result.getX1()+";"+result.getY1()+"),("+result.getX2()+";"+result.getY2()+")]");	
		if (result == null) throw new Exception("No destLine found");
		return result;
	}

	private Point2D getDestPoint(Point2D robot,Line2D destLine) {
		Point2D result = null;
		
		if (Base.DEBUG) System.out.println("(getDestPoint) Distance Robot/Line = "+destLine.ptLineDist(robot));
		if (destLine.ptLineDist(robot) > 1){	//Robot is far away from the line, go to it first
			if (Base.DEBUG) System.out.println("(getDestPoint) Far from the line");
			result = this.project(robot, destLine);
		} else {		//Robot is close to or on the line, go to the right bound
			if (Base.DEBUG) System.out.println("(getDestPoint) Close to the line");
			result = destLine.getP2();
		}
		if (Base.DEBUG) System.out.println("(getDestPoint) result is "+result);
		return result;
	}

	private Vector<Point2D[]> getPolygons() throws Exception {
		// Keep track of sites done
        HashSet<Pnt> done = new HashSet<Pnt>(initialTriangle);
        Vector<Point2D[]> polygons = new Vector<Point2D[]>(10,0);
        
        for (Triangle triangle : dt){
            for (Pnt site: triangle) {
                if (done.contains(site)) continue;
                done.add(site);
                List<Triangle> list = dt.surroundingTriangles(site, triangle);
                Point2D[] vertices = new Point2D[list.size()];		//vertices == polygon
                int i = 0;
                for (Triangle tri: list){
                    vertices[i++] = new Point2D.Double(tri.getCircumcenter().coord(0),tri.getCircumcenter().coord(1));
                }
                polygons.add(vertices);
            }
        }
        if (polygons.size() == 0) throw new Exception("Cannot find any polygons");
		return (polygons);
	}

	private void genVoronoiDiag(Vector<double[]> landmarks) {
		Triangle triangle;
		Pnt point;
		int i;
		
		//Generate Voronoi Diagram
		for(i=0; i < landmarks.size(); i++){
			point = new Pnt(landmarks.get(i)[1], landmarks.get(i)[2]);
			triangle = dt.locate(point);
			if (triangle == null) throw new IllegalArgumentException("No containing triangle");
			if (!triangle.contains(point)){		//point is not in the triangulation => recreate the whole diagram
				if (Base.DEBUG) System.out.println("(genVoronoiDiag) Landmark " + i + ", on "+ landmarks.size() +", is not in the diagram");
				dt = new Triangulation(initialTriangle);	//Flush the triangulation
				this.polygons = null;						//Flush current polygons
				for(int j=0; j < landmarks.size(); j++){	//Add all new landmarks
					point = new Pnt(landmarks.get(j)[1], landmarks.get(j)[2]);
					dt.delaunayPlace(point);
				}
				break;
			}	//else : ok, look other landmarks
		}
		
		if (i == landmarks.size()) {	// no new landmark
			if (Base.DEBUG) System.out.println("(genVoronoiDiag) No need to regenerate diagram");
		} else {
			if (Base.DEBUG) System.out.println("(genVoronoiDiag) We generate a " + dt);
		}
	}
	
	public double[] getNextPoint(Landmarks landmarks, double x_robot, double y_robot, double r_robot) throws Exception{
		Point2D dest = null;
		Point2D robot = new Point2D.Double(x_robot,y_robot);
		double[] result= new double[2]; 
		double [] tempLandmark = null;
		ListIterator<double[]> li;
		int nbCloseLandmarks = 0;
		
		if (Base.DEBUG) System.out.println("(getNextPoint) Robot : ("+x_robot+","+y_robot+"), r="+r_robot);
		
		//Calculate the number of landmark which the robot is in their communication range	
		li = landmarks.getLandmarks().listIterator();
		double[] closeLandmark = null;
        while(li.hasNext()){
        	tempLandmark = li.next();
        	if (this.euclideanDist(tempLandmark[1], tempLandmark[2], x_robot, y_robot)<=DataChecker.communicationRange+1){
        		nbCloseLandmarks++;
        		closeLandmark = tempLandmark;
        	}
        }
        
        //Find next destination
        if (nbCloseLandmarks == 0){			//Far away from all landmark, go to the final destination
        	if (Base.DEBUG) System.out.println("(getNextPoint) No close landmark, go to ("+Base.finalDestination[0]+","+Base.finalDestination[1]+")");
			result = Base.finalDestination;
        } else if (nbCloseLandmarks == 1){	//Only one landmark close, follow the communication range
			if (Base.DEBUG) System.out.println("(getNextPoint) Only landmark close"+(int)closeLandmark[0]+" ("+closeLandmark[1]+","+closeLandmark[2]+")");
			double distance = 1, alpha, beta, gamma;
			//double[] landmark1 = landmarks.getLandmarks().get(0);
			double distEucl = this.euclideanDist(x_robot,y_robot,closeLandmark[1],closeLandmark[2]);
			alpha = Math.acos(distance/(2*distEucl));
			beta = Math.atan((closeLandmark[2]-y_robot)/(closeLandmark[1]-x_robot));
			gamma = beta-alpha;
			if (Base.DEBUG) System.out.println("(getNextPoint) Euclidian Distance = "+distEucl+", alpha = "+alpha+", beta = "+beta+", gamma = "+gamma);
			result[0] = distance * Math.cos(gamma) + x_robot;
			result[1] = distance * Math.sin(gamma) + y_robot;
			
		} else {					//Several landmarks, generate Voronoi Diagram then follow it
			if (Base.DEBUG) System.out.println("(getNextPoint) "+ landmarks.size()+" landmarks");
			this.genVoronoiDiag(landmarks.getLandmarks());
			if (this.polygons==null) this.polygons = this.getPolygons();
			Line2D destLine = this.getDestLine(robot, this.polygons);
			dest = this.getDestPoint(robot, destLine);
			
			//return destination coordinates in a double[]
			result[0] = dest.getX();
			result[1] = dest.getY();	
		}
		if (Base.DEBUG) System.out.println("(getNextPoint) DestPnt = ("+result[0]+";"+result[1]+")");
		
		return(result);
	}
}
